%% Configuration
  dt = .002 ; % Time step
  T = 200*dt; % 300 Total time
  Niter = round(T/dt); % Numberof interations
   
  Ns = 4;  % 10 Number of segments in the chain
  Ls = 1; % Length of the segments;
  K = 1500 ;  % Spring constant representing the link between masses
  BETA = 20*dt; % Disipation
  m = (100/(Ns+1))*ones(1,Ns+1);
%  m(1)=100*m(1);
  
  % Angular Potential
  % Is a funtion with the shape f(x,param)
  % With param a vector of parameters;
%  f=@DoubleWell_circ; % Function name (handle)
%  E=20; % Energy between the minima
%  x0=1/sqrt(2); % Position of the minima
%  param=[E x0];
  
  f=@VertRest; 
  kv=500.0; % Vertical spring constant
  param(1)=kv;
  
%% Intial Conditions
  initang = (270/Ns)*ones(1,Ns+1)./linspace(4,1,Ns+1);%(180/Ns)*zeros(1,Ns+1);% 180/Ns
  initang(1:2)=0;
%  initang=zeros(1,Ns+1);
  r0 = Ls.*set_init(initang);
  r_prev=r0; % Initialization of integration scheme
  r=r0;
  mid0=mean(r(1,:)); % For the plot
  
%% Temporal integration
  r_versor=zeros(2,Ns);
  ang_versor=r_versor;
  for i=1:Niter
  % The oktopus -- K
    if(i==1)
      disp('Press enter to send the pulse')
      pause();
    end
    param(1)=kv;
	if( i*dt<300e-3 )
	   param(1)=kv*(1-exp(-(i*dt)/300e-3));
	end
  
    %% Geometrical calculations
  	  r_versor = r(:,2:Ns+1)-r(:,1:Ns);
  	  dr = sqrt(sumsq(r_versor))-Ls;  % Deformations
  	  r_versor(1,:) = r_versor(1,:)./(dr+Ls); % Radial unit vectors
  	  r_versor(2,:) = r_versor(2,:)./(dr+Ls);
  	  ang_versor(1,:) = -r_versor(2,:);  % Angular unit vectors
  	  ang_versor(2,:) = r_versor(1,:);
  	
  	%% Force calculation
  	
  	   % Force in the radial component
  	   % Left most segment and second from the left
  	   	 Fr(:,1) = K*dr(1)*r_versor(:,1)/100;
  	   	 Fr(:,2) = K* ( dr(2)*r_versor(:,2)- dr(1)*r_versor(:,1) )/100;
  	   
  	   % middle segments
  	     aux=repmat(dr,2,1);
  	     Fr(:,3:Ns-1) = K*(  aux(:,3:Ns-1).*r_versor(:,3:Ns-1) ...
  	                        -aux(:,2:Ns-2).*r_versor(:,2:Ns-2) );

  	   % Right most segment and second from the right
  	   	 Fr(:,Ns) = K*( dr(Ns)*r_versor(:,Ns)-dr(Ns-1)*r_versor(:,Ns-1) );
  	   	 Fr(:,Ns+1) = -K*dr(Ns)*r_versor(:,Ns);
  	   	 
  	   % Force due to the angular potential
  	   % Will call f with dsin as the argument.
  	   dsin = r_versor(1,1:Ns-1).*r_versor(2,2:Ns)-r_versor(2,1:Ns-1).*r_versor(1,2:Ns);  % sin(ang2 - ang1)
  	   Fang=Fpotential(r_versor,ang_versor,f,dsin,param);
  	

% Plot
    if (i==1) 
        plot(r(1,:),r(2,:),'o-',r(1,1),r(2,1),'og');
        line([0 0],[-10 10])
        axis( [min(r(1,:))-Ls max(r(1,:))+Ls -10 10],"square")
       % axis( [mid0-(Ns+2)*Ls/2 mid0+(Ns+2)*Ls/2 -10 10],"square")
        pause(.0001);
    end    

% Logging


% The oktopus -- Force
%    if(i==1)
%      disp('Press enter to send the pulse')
%      pause();
%    end
%	if( i*dt<250e-3 )
%		Fr(:,2)=Fr(:,2)+2*[sqrt(10); 0];
%	elseif( i*dt>250e-3 && i*dt<500e-3 )
%		Fr(:,2)=Fr(:,2)+[-sqrt(10); sqrt(10)];
%	end
	
% Verlet Algorithm without tracking velocities
    r_swap=r;
    r=(2-BETA)*r-(1-BETA)*r_prev + dt*(Fr+Fang)./[m;m];
    r_prev=r_swap;

%% ERASE HERE :D    
% The oktopus -- Position
%    DeltaT=50e-3;
%	if( i*dt<DeltaT)
%    r(:,2)=[r0(1,2)+.08*sin(2*pi*i*dt/DeltaT);r_prev(2,2)];
%    end
	r(:,1)=r_prev(:,1);

% Plot
    if (mod(i*dt,1e-3)==0) 
        plot(r(1,:),r(2,:),'o-',r(1,1),r(2,1),'og');
        line([0 0],[-10 10])
        title(['Time: ' num2str(i*dt) ' s'])
%        axis( [min(r(1,:))-Ls max(r(1,:))+Ls -10 10],"square")
        axis( [mid0-(Ns+2)*Ls/2 mid0+(Ns+2)*Ls/2 -2 20],"square")
        pause(.0001);
    end    

end
